#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     mux,                 sensorI2CCustom)
#pragma config(Sensor, S3,     IRmux,               sensorI2CCustom)
#pragma config(Sensor, S4,     gyro,                sensorI2CHiTechnicGyro)
#pragma config(Motor,  motorA,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorB,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorC,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     frontr,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     backr,         tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     frontl,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     backl,         tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    d1,                   tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    cup1,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    lg1,                  tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    rg1,                  tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_1,    d2,                   tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    cup2,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    lg2,                  tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    rg2,                  tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    servo11,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo12,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int diff;

#include "include.h"

task motorrun()
{while(true){
motor[motorB]=100;
wait10Msec(300);
motor[motorB]=-100;
wait10Msec(50);}}
task main()
{
  initializeRobot();

  waitForStart(); // Wait for the beginning of autonomous phase.

wait10Msec(wait*100);
gyroHeading=0;
      servo[lg1]=0;
servo[lg2]=255;
  switch(automode)
{
  case leftdump:
  leftDump();
 straightdist(-35,900,0,0);
  while(abs(gyroGetHeading())<85){
  left(25);}
  break;

  case rightdump:
  rightDump();
  straightdist(-35,900,0,0);
  while(abs(gyroGetHeading())<85){
  right(25);}
  break;

  case leftnodump:
  noDump();
 straightdist(-35,900,0,0);
  while(abs(gyroGetHeading())<85){
  left(25);}
  break;

  case rightnodump:
  noDump();
  straightdist(-35,900,0,0);
  while(abs(gyroGetHeading())<85){
  right(25);}
  break;

}
straight(50,0,0);
wait10Msec(550);
close();
int farfromhere=90;
while  (USreadDist(msensor_S2_1)>farfromhere&&USreadDist(msensor_S2_2)>farfromhere)
{
  if(USreadDist(msensor_S2_1)>farfromhere&&USreadDist(msensor_S2_2)>farfromhere)
  {turn(30,30);}
    if(USreadDist(msensor_S2_1)<farfromhere&&USreadDist(msensor_S2_2)>farfromhere)
  {turn(30,0);}
    if(USreadDist(msensor_S2_1)>farfromhere&&USreadDist(msensor_S2_2)<farfromhere)
  {turn(0,30);}
   }

stop();
gyroHeading=0;
wait10Msec(100);
switch(automode){
  case rightdump:
   while(abs(gyroGetHeading())<85)
  {right(20);}
 break;
case rightnodump:
   while(abs(gyroGetHeading())<85)
  {right(20);}
 break;
  case leftdump:
   while(abs(gyroGetHeading())<88)
  {left(20);}
 break;
case leftnodump:
   while(abs(gyroGetHeading())<88)
  {left(20);}
 break;

}
 StartTask(motorrun);
gyroHeading=0;
while(true)
{

  if(TSreadState(msensor_S2_3)==0&&TSreadState(msensor_S2_4)==0)
  {TSmode=neither;}
  if(TSreadState(msensor_S2_3)==1&&TSreadState(msensor_S2_4)==0)
  {TSmode=only3;}
  if(TSreadState(msensor_S2_3)==0&&TSreadState(msensor_S2_4)==1)
  {TSmode=only4;}
  if(TSreadState(msensor_S2_3)==1&&TSreadState(msensor_S2_4)==1)
  {TSmode=both;}

        switch(TSmode){
      case neither:
       straight(30,0,0);

      break;

      case only3:
      turn(0,30);


      break;

      case only4:
      turn(30,0);


      break;

      case both:
      stop();

      break;
    }
  }

}
